// A1_Farbkanaele_trennen.cpp : Definiert den Einstiegspunkt für die Konsolenanwendung.

#include <fstream>
#include <stdio.h>
#include <iostream>
#include "opencv/cv.h"
#include "opencv/highgui.h"
#include "opencv/cxcore.h"
#include <math.h>
#include <windows.h>
#include <time.h>
#include <ctime>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>

#define  BUFSIZE 256
#define USE_MEAN
#define DEBUGGING
#define USE_CAMERA

using namespace cv;
using namespace std;

#ifdef USE_CAMERA
//320×240 @ 120fps
//640×480 @ 60 fps
//	#define xres 320
//	#define yres 240
	#define xres 640
	#define yres 480
#else
	#define xres 360
	#define yres 480
#endif

//Nicht genug Platz auf dem Stack...
	/*
	unsigned int I1[xres][yres];
	unsigned int S[xres][yres];
	unsigned int I2[xres][yres];
	unsigned int Q[xres][yres];
	float results[xres][yres];
	*/

//! Mats mit 32FC1 verwenden
	//float I1[xres][yres];
	//float S[xres][yres];
	//float I2[xres][yres];
	//float Q[xres][yres];
	//float results[xres][yres];

	//int P[50][62];
	//int P2[50][62];

//Output Fenster
char winPictureName[] = "Bottle Picture";
char winPatternName[] = "Pattern Picture";
char winResultName[] = "Result Picture";
char winIntegralName[] = "Integration of Picture";
char winIntegral2Name[] = "Integration of Quadrats";
char winResultReplaceName[] = "Result Replace Picture";
char winCam[] = "Camera direct output";

//Calculates the Class Mean Vektor
Mat calcMeanVector(Mat classMat)
{
	int cols = classMat.cols;
	int rows = classMat.rows;
	Mat mean(1,cols,CV_32F);
	for(int i=0; i<cols; i++)
	{
		double value = 0;
		for(int j=0; j<rows; j++)
		{
			value += classMat.at<float>(j,i);
		}
		mean.at<float>(0,i) = (value/rows);
	}
	return mean;
}
void saveMat(Mat samples,Mat labels,string filename)
{
	fstream f;
    f.open(filename, ios::out);
    for(int i = 0;i<samples.rows;i++)
	{
		for(int j = 0;j<samples.cols;j++)
		{
			f << samples.at<float>(i,j)<<",";
		}
		f<<labels.at<int>(i);
		f<<endl;
	}
    f.close();
}
Mat asignColor(Mat imgIn,Mat centers)
{
	Mat imgIn2 = imgIn.clone();

	Mat samples2(imgIn2.rows*imgIn2.cols,3,CV_32F);	
	
	MatIterator_<Vec3b> it2 = imgIn2.begin<Vec3b>();
	for(int i = 0;it2<imgIn2.end<Vec3b>();++it2,++i)
	{
		samples2.at<float>(i,0) = (*it2)[0]; // Pixel B
		samples2.at<float>(i,1) = (*it2)[1]; // Pixel G
		samples2.at<float>(i,2) = (*it2)[2]; // Pixel R
	}
	Mat labels2;// nextProto(samples2,centers);

	//Do new color asignment
	it2 = imgIn2.begin<Vec3b>();
	for(int i = 0;it2<imgIn2.end<Vec3b>();++it2,++i)
	{
		int NrLabel = labels2.at<int>(i,0);
		(*it2)[0] =  centers.at<float>(NrLabel,0);// Pixel B
		(*it2)[1] =  centers.at<float>(NrLabel,1); // Pixel G
		(*it2)[2] =  centers.at<float>(NrLabel,2); // Pixel R
	}
	return imgIn2;
}


Point getMaxPosition(Mat result,Point rangeUL,Point rangeLR)
{
	Point maxPoint;
	uint32_t max = 0;
	for(uint32_t x = rangeUL.x; x<rangeLR.x;x++)
	{
		for(uint32_t y = rangeUL.y;y<rangeLR.y;y++)
		{
			if(result.at<uchar>(y,x) > max)
			{
				max = result.at<uchar>(y,x);
				maxPoint = Point(x,y);
			}

		}
	}
	return maxPoint;
}
void copyMatToPosition(Mat pic,Mat pattern,Point pos)
{
	uint32_t x_pattern = 0;
	uint32_t y_pattern = 0;

	for(uint32_t x = pos.x; x < (pos.x + pattern.cols);x++,x_pattern++)
	{
		for(uint32_t y = pos.y , y_pattern = 0; y < (pos.y + pattern.rows);y++,y_pattern++)
		{
			pic.at<uchar>(y,x) = pattern.at<uchar>(y_pattern,x_pattern);
		}
	}
}
void drawPatternFrame(Mat pic,Mat pattern,Point maxPoint)
{
	Point rectUL = maxPoint;
	Point rectLR = Point(maxPoint.x + pattern.cols, maxPoint.y + pattern.rows);
	rectangle(pic,rectUL,rectLR,Scalar(0,0,255),1);
}
void diffclock(clock_t clock1,clock_t clock2, uint32_t* sec,uint32_t* ms)
{
	uint32_t diffticks=clock1-clock2;
	(*sec) = diffticks/CLOCKS_PER_SEC;
	(*ms) = diffticks%CLOCKS_PER_SEC;
}
float calcPatternMean(Mat pattern)
{
	float patternMean = 0.0;
	for(uint32_t x_pattern = 0; x_pattern< pattern.cols;x_pattern++)
	{
		for(uint32_t y_pattern = 0; y_pattern<pattern.rows;y_pattern++)
		{
			//patternMean += (float)((uchar*)(pattern.data + pattern.step*y_pattern))[x_pattern];
			patternMean += (float)pattern.ptr<uchar>(y_pattern)[x_pattern];
		}	
	}
	patternMean /= (float)(pattern.rows*pattern.cols);
	return patternMean;
}
uint32_t calcST(Mat pattern, uint32_t mean)
{
	uint32_t ST = 0;
	for(uint32_t x_pattern = 0; x_pattern< pattern.cols;x_pattern++)
	{
		for(uint32_t y_pattern = 0; y_pattern<pattern.rows;y_pattern++)
		{
			ST += ((((uchar*)(pattern.data + pattern.step*y_pattern))[x_pattern])-mean)^2;
		}	
	}
	ST /= (pattern.rows*pattern.cols);
	return ST;
}



//in use------------------------------------------------------------------------------------------------------
float calcS_T(Mat pattern, int N)
{
	int sum = 0;
	for(int x =0; x<pattern.cols; x++)
	for(int y = 0; y<pattern.rows; y++)
	{
		//pow() ist zu stark überladen, deswegen:
		int val=pattern.ptr<float>(y)[x];
		sum += val*val;				//Gebildet aus der Summe der Quadratischen Werte vom Pattern
	}
	return sqrt((float)sum/(N));
}
inline Mat calcI(Mat picture)
{
	Mat Sm(picture.rows, picture.cols, CV_64FC1);
	Mat I(picture.rows, picture.cols, CV_64FC1);

	//Berechnung des einfachen Integralbildes
	for(int x = 0; x < picture.cols; x++)					//Übernahme der ersten Zeile nach S (Zwischensumme) zur Vermeidung von falschen Pointerzugriffen
		Sm.ptr<double>(0)[x] = picture.ptr<uchar>(0)[x];
	for(int y = 1; y < picture.rows; y++)					//Berechnung von S, integration in y-Richtung
		for(int x = 0; x < picture.cols; x++)
			Sm.ptr<double>(y)[x] = picture.ptr<uchar>(y)[x]+Sm.ptr<double>(y-1)[x];
	for(int y = 0; y < picture.rows; y++)					//Übernahme der ersten Spalte von S nach I (Integralbild)
		I.ptr<double>(y)[0] =Sm.ptr<double>(y)[0];								
	for(int x = 1; x < picture.cols; x++)					//Berechnung von I, integration in x-Richtung
		for(int y = 0; y < picture.rows; y++)
			I.ptr<double>(y)[x] = I.ptr<double>(y)[x-1]+Sm.ptr<double>(y)[x];
	return I;
}
inline Mat calcI2(Mat picture)
{
	Mat Sm(picture.rows, picture.cols, CV_64FC1);
	Mat I2(picture.rows, picture.cols, CV_64FC1);
	Mat Qm(picture.rows, picture.cols, CV_64FC1);

	//Berechnung des Integralbildes der Quadrate von F (Originalbild)
	for(int y = 0; y < picture.rows; y++)					//Berechnung der Quadrate vom Originalbild nach Q
		for(int x = 0; x < picture.cols; x++)
		{
			int val = picture.ptr<uchar>(y)[x];
			Qm.ptr<double>(y)[x]= (double)val*val;
		}
	//Same Procedure than last Year... bei calcI, die Quelle ist aber Q und das Ziel I2
	for(int x = 0; x < picture.cols; x++)				//Übernahme der ersten Zeile nach S (Zwischensumme, wiederverwendet) zur Vermeidung von falschen Pointerzugriffen
		Sm.ptr<double>(0)[x] = Qm.ptr<double>(0)[x];
	for(int y = 1; y < picture.rows; y++)					//Berechnung von S , integration in y-Richtung
		for(int x = 0; x < picture.cols; x++)
			Sm.ptr<double>(y)[x] = Qm.ptr<double>(y)[x]+Sm.ptr<double>(y-1)[x];
	for(int y = 0; y < picture.rows; y++)					//Übernahme der ersten Spalte von S nach I (Integralbild)
		I2.ptr<double>(y)[0] =Sm.ptr<double>(y)[0];								
	for(int x = 1; x < picture.cols; x++)					//Berechnung von I, integration in x-Richtung
		for(int y = 0; y < picture.rows; y++)
			I2.ptr<double>(y)[x] = I2.ptr<double>(y)[x-1]+Sm.ptr<double>(y)[x];
	return I2;
}
Mat shiftMat(Mat pattern,float patternMean)
{
	Mat shift(pattern.rows,pattern.cols,CV_32FC1);
	for(int x=0; x<pattern.cols; x++)
		for(int y = 0; y<pattern.rows; y++)
		{
			float val = (float)pattern.ptr<uchar>(y)[x]-patternMean;
			shift.ptr<float>(y)[x] = val;
		}
			

	return shift;
}


Mat intXCorr(Mat pattern, Mat pic, float varianzGrenze)
{
	Mat I(pic.rows, pic.cols,CV_32FC1);
	Mat I2 = I.clone();
	Mat result = pic.clone();
	//Vorberechnung für jedes zu untersuchende Bild
	//Integralbilder
	Mat Fsum(pic.rows, pic.cols, CV_8UC1);
	Mat F2sum(pic.rows, pic.cols, CV_8UC1);
	
	//Vorberechnung für das zu suchende Muster
	float T_bar = 0.0;
	float N = 0;

	//Berechnung der konstanten Faktoren für das Pattern
	//Patterngröße
	N = pattern.rows * pattern.cols;
	float patternMean = calcPatternMean(pattern);

	Mat P = shiftMat(pattern, patternMean);
	
	//S_T für die Normierung aufgrund der Patternwerte
	float S_T = calcS_T(P, N);

	//Berechnung der Integralbilder von F und F^2
	I = calcI(pic); 
	I2 = calcI2(pic);

	for(int rx = 1;rx<(pic.cols-pattern.cols-1);rx++)
		{
			for(int sy = 1;sy<(pic.rows-pattern.rows-1);sy++)
			{
					//Berechnung von S_F (S_T ist schon bekannt und konstant)
					//Auslesen der Summe der Quadratischen Werte
					int dx = -1;
					int dy = -1;

					//Auslesen der Summe der Quadratischen Werte
					double sumF2 =I2.ptr<double>(sy+P.rows+dy)[rx+P.cols+dx] - I2.ptr<double>(sy+P.rows+dy)[rx+dx]-	I2.ptr<double>(sy+dy)[rx+P.cols+dx]+I2.ptr<double>(sy+dy)[rx+dx];
					//Auslesen der Summe der Bild-Werte
					double sumF =I.ptr<double>(sy+P.rows+dy)[rx+P.cols+dx] - I.ptr<double>(sy+P.rows+dy)[rx+dx]-	I.ptr<double>(sy+dy)[rx+P.cols+dx]+I.ptr<double>(sy+dy)[rx+dx];

					double S_F = sqrt(sumF2/N-sumF*sumF/(N*N));

				if(S_F>varianzGrenze)//Numerischen Problemen vorbeugen
				{
					float Zsum_ = 0;
					for(int x = 0; x < P.cols; x++)
						for(int y = 0; y < P.rows; y++)
						{//Kritische Stelle
							Zsum_ +=	pic.ptr<uchar>(y+sy)[x+rx]*P.ptr<float>(y)[x];
						}
					float Zsum = Zsum_/(float)N;

					//Vorbereitung Ende, Berechnung des Koeffizienten
					double h=Zsum/(S_F*S_T);

				if(h>0.0)
					result.ptr<uchar>(sy)[rx] = (int)(h*255.0);
				else
					result.ptr<uchar>(sy)[rx] = 0;
				}
				else
				{
					result.ptr<uchar>(sy)[rx]=0;
				}
			}
		}
	return result;
}

//NOT used------------------------------------------------------------------------------------------------------
_forceinline float calcCorrelationCoefficient_Integral(Mat picture, Mat pattern, Mat I,Mat I2,int rx,int sy, int N, double S_T)
{
	//Berechnung der optimierten Kreuzkorrelation mit den Integralbildern
	//*ACHTUNG!:*rx,sy > pattern x,y ; ansonsten resultiert falscher Zugriff auf I1, I2 (Bsp: I1[-1][-1] möglich)
	//VERBESSERUNG: Muster in I verschieben um +px, +py und die neuen Zellen mit 0 auffüllen.
	//=> Kein Pointer Problem und! in der Summenberechnung fliegen die -1 therme raus.

	//Berechnung der Faltung im Zähler
	int Zsum_ = 0;
	for(int x = 0; x < pattern.cols; x++)
		for(int y = 0; y < pattern.rows; y++)
		{
			Zsum_ +=	picture.ptr<uchar>(y+sy)[x+rx]*pattern.ptr<int>(y)[x];
		}
	float Zsum = Zsum_/N;
		
	//Berechnung von S_F (S_T ist schon bekannt und konstant)

	//Auslesen der Summe der Quadratischen Werte
	float sumF2 =I2.ptr<float>(sy+pattern.rows-1)[rx+pattern.cols-1] - I2.ptr<float>(sy+pattern.rows-1)[rx-1]-	I2.ptr<float>(sy-1)[rx+pattern.cols-1]+I2.ptr<float>(sy-1)[rx-1];
	//Auslesen der Summe der Bild-Werte
	float sumF = I.ptr<float>(sy+pattern.rows-1)[rx+pattern.cols-1] - I.ptr<float>(sy+pattern.rows-1)[rx-1]-	I.ptr<float>(sy-1)[rx+pattern.cols-1]+I.ptr<float>(sy-1)[rx-1];
	float S_F = sqrt(sumF2/N-sumF*sumF/N/N);
	//Vorbereitung Ende, Berechnung des Koeffizienten
	float h=Zsum/(S_F*S_T);

	return h;
}




//end NOT used------------------------------------------------------------------------------------------------------
int main(void)
{
	// Bilddatei einlesen

	char fname[] = "Flasche_05.jpg";

	Mat pattern = imread("Muster_.jpg",CV_8UC1);
		if (!pattern.data) exit(-3);

	Mat sub1 = imread("1.jpg",CV_8UC1);
		if (!sub1.data) exit(-3);
	Mat sub2 = imread("2.jpg",CV_8UC1);
		if (!sub2.data) exit(-3);
	Mat sub3 = imread("3.jpg",CV_8UC1);
		if (!sub3.data) exit(-3);
	Mat sub4 = imread("4.jpg",CV_8UC1);
		if (!sub4.data) exit(-3);

#ifndef USE_CAMERA
	Mat pic = imread(fname,CV_8UC1);
		if (!pic.data) exit(-3);
	Mat picColor = imread(fname);
		if (!picColor.data) exit(-3); 
#else	
	VideoCapture cam(1); // open the default camera
    if(!cam.isOpened())  // check if we succeeded
        return -5;
	Mat picColor;
	Mat pic;
	cam >> picColor;
	cvtColor(picColor,pic,CV_BGR2GRAY);
	cvWaitKey(0);

#endif

	namedWindow(winPictureName,CV_WINDOW_AUTOSIZE);
	namedWindow(winPatternName,CV_WINDOW_AUTOSIZE);
	namedWindow(winResultName,CV_WINDOW_AUTOSIZE);
	namedWindow(winResultReplaceName,CV_WINDOW_AUTOSIZE);
	//namedWindow(winIntegralName,CV_WINDOW_AUTOSIZE);
	//namedWindow(winIntegral2Name,CV_WINDOW_AUTOSIZE);
	//namedWindow(winCam,CV_WINDOW_AUTOSIZE);

#ifdef USE_CAMERA
	while(1)
	{
	cam >> picColor;
	cvtColor(picColor,pic,CV_BGR2GRAY);
		
#endif
	Mat resultReplace = pic.clone();
	Mat result;
	Mat buff;
	Point maxPoint;
	//clock_t start = clock();
	

	float varianzGrenze = 15.0;

	//Finde Teil 1
	buff = sub1.clone();
	result = intXCorr(buff,pic,varianzGrenze);
	maxPoint = getMaxPosition(result, Point(1,1),Point(pic.cols-buff.cols-1,pic.rows-buff.rows-1));
	copyMatToPosition(resultReplace,buff,maxPoint);
	drawPatternFrame(picColor,buff,maxPoint);

	//Finde Teil 2
	buff = sub2.clone();
	result = intXCorr(buff,pic,varianzGrenze);
	maxPoint = getMaxPosition(result, Point(1,1),Point(pic.cols-buff.cols-1,pic.rows-buff.rows-1));
	copyMatToPosition(resultReplace,buff,maxPoint);
	drawPatternFrame(picColor,buff,maxPoint);

	//Finde Teil 3
	buff = sub3.clone();
	result = intXCorr(buff,pic,varianzGrenze);
	maxPoint = getMaxPosition(result, Point(1,1),Point(pic.cols-buff.cols-1,pic.rows-buff.rows-1));
	copyMatToPosition(resultReplace,buff,maxPoint);
	drawPatternFrame(picColor,buff,maxPoint);

	//Finde Teil 4
	buff = sub4.clone();
	result = intXCorr(buff,pic,varianzGrenze);
	maxPoint = getMaxPosition(result, Point(1,1),Point(pic.cols-buff.cols-1,pic.rows-buff.rows-1));
	copyMatToPosition(resultReplace,buff,maxPoint);
	drawPatternFrame(picColor,buff,maxPoint);

	//clock_t end = clock();


	//imshow(winIntegralName,Fsum);
	imshow(winResultReplaceName,resultReplace);
	imshow(winResultName,result);
	imshow(winPatternName,pattern);
	//imshow(winIntegral2Name,F2sum);
	imshow(winPictureName,picColor);
	//namedWindow("cam",CV_WINDOW_AUTOSIZE);
	cvWaitKey(1);
#ifdef USE_CAMERA
	}
#endif

//	imwrite("replace.jpg",resultReplace);

	uint32_t sec,ms;
	//diffclock(end,start,&sec,&ms);
	cout<<"Execution time: "<<sec<<"."<<ms<<" sec"<<endl;

	
	cvWaitKey(0);
return 0;
}

	// Skalierung und Beschreiben der Integralbilder
	/*
	double max = 0.0;
	double scal = 0.0;
	int x; int y;
	for(int x = 0; x < pic.cols; x++)
		for(int y = 0; y<pic.rows;y++)
			if (I.ptr<double>(y)[x]>max)
				max = I.ptr<double>(y)[x];
	scal = 1/max*255;
	for(int x = 0; x < pic.cols; x++)
		for(int y = 0; y<pic.rows;y++)
			Fsum.ptr<uchar>(y)[x] = I.ptr<double>(y)[x]*scal;
			
	max = 0.0; scal = 0.0;
	for(int x = 0; x < pic.cols; x++)
		for(int y = 0; y<pic.rows;y++)
			if (I2.ptr<double>(y)[x]>max)
				max = I2.ptr<double>(y)[x];
	cout<< "max: " << max<<endl;
	scal = 1/max*255;
	for(int x = 0; x < pic.cols; x++)
		for(int y = 0; y<pic.rows;y++)
			F2sum.ptr<uchar>(y)[x] = I2.ptr<double>(y)[x]*scal;
*/